#include "sbus.h"
#include <BLE2902.h>
#include <BLEDevice.h>
#include <BLEServer.h>
#include <BLEUtils.h>

#define DEBUG

#define DEBUG_BLE_SETUP() Serial.println("[BLE] 初始化蓝牙设备")
#define DEBUG_BLE_CONNECT() Serial.println("[BLE] 客户端已连接")
#define DEBUG_BLE_DISCONNECT() Serial.println("[BLE] 客户端已断开连接")
#define DEBUG_BLE_WRITE(data) Serial.printf("[BLE] 收到数据: %s\n", data)
#define DEBUG_BLE_NOTIFY() Serial.println("[BLE] 发送状态通知")
#define DEBUG_BLE_ERROR(msg) Serial.printf("[BLE] 错误: %s\n", msg)
#define SERVICE_UUID "9b574847-f706-436c-bed7-fc01eb0965c1"
#define CHARACTERISTIC_UUID "681285a6-247f-48c6-80ad-68c3dce18585"

int numbers[5];
int data_to_send[5] = {988, 991, 988, 992, 0};
int receivedCount = 0;

bfs::SbusTx sbus_tx(&Serial1, 1, 2, true);
bfs::SbusData data;

bool D_UP_Flag = 0;
bool D_DOWN_Flag = 0;
bool START_Flag = 0;

// DOG State Flags:
bool HIGH_STAND_Flag = 1;

BLEServer *pServer = nullptr;
BLECharacteristic *pCharacteristic = nullptr;
bool deviceConnected = false;

struct SBUS_DATA {
  int32_t Lx;
  int32_t Ly;
  int32_t Rx;
  int32_t Ry;
  int32_t CH5;
  int32_t CH6;
  int32_t CH7;
  int32_t CH8;
};

int parseNumbers(String input, int *output) {
  int count = 0;
  int startIndex = 0;
  int endIndex = 0;

  while (endIndex >= 0 && count < 4) {
    endIndex = input.indexOf(',', startIndex);
    if (endIndex == -1) {
      endIndex = input.indexOf(' ', startIndex);
    }

    String numberStr;
    if (endIndex >= 0) {
      numberStr = input.substring(startIndex, endIndex);
    } else {
      numberStr = input.substring(startIndex);
    }

    numberStr.trim();
    if (numberStr.length() > 0) {
      output[count] = numberStr.toInt();
      count++;
    }

    startIndex = endIndex + 1;
  }

  return count;
}

void writeSBUS(SBUS_DATA my_data) {

  data.ch[0] = my_data.Rx;
  data.ch[1] = my_data.Ry;
  data.ch[2] = my_data.Ly;
  data.ch[3] = my_data.Lx;
  data.ch[4] = my_data.CH5; // CH5 default 992
  data.ch[5] = my_data.CH6; // CH6
  data.ch[6] = my_data.CH7; // CH7
  data.ch[7] = my_data.CH8; // CH8
  data.ch[8] = 1690;
  data.ch[9] = 154;

  sbus_tx.data(data);
  sbus_tx.Write();
}

void sendCommand(int16_t Lx, int16_t Ly, int16_t Rx, int16_t Ry,
                 bool START = true, bool DPAD_UP = true,
                 bool DPAD_DOWN = false) {
  // MODE FLAGS:
  int32_t MODE1 = 992;
  int32_t MODE2 = 192;
  int32_t MODE3 = 1792;

  int32_t _LOW = 192;
  int32_t _MID = 992;
  int32_t _HIGH = 1792;

  int32_t CH5_STATE = MODE1;
  int32_t CH6_STATE = _LOW;
  int32_t CH7_STATE = _LOW;
  int32_t CH8_STATE = _MID;

  // Store Values Here to write to SBUS TX
  SBUS_DATA tx_data;

  tx_data.Lx = Lx;
  tx_data.Ly = Ly;
  tx_data.Rx = Rx;
  tx_data.Ry = Ry;
  tx_data.CH5 = CH5_STATE;
  tx_data.CH6 = CH6_STATE;
  tx_data.CH7 = CH7_STATE;
  tx_data.CH8 = CH8_STATE;

  // START BUTTON - Unlock
  if (START == true) {
    if (START_Flag == 0) {
      Serial.println("Unlocked");
      CH5_STATE = MODE1;
      CH7_STATE = _LOW;
      tx_data.CH5 = CH5_STATE;
      tx_data.CH7 = CH7_STATE;

      writeSBUS(tx_data);
      delayMicroseconds(600);
      ;

      CH5_STATE = MODE1;
      CH7_STATE = _HIGH;
      tx_data.CH5 = CH5_STATE;
      tx_data.CH7 = CH7_STATE;

      writeSBUS(tx_data);
      delayMicroseconds(600);
      ;

      CH5_STATE = MODE1;
      CH7_STATE = _LOW;
      tx_data.CH5 = CH5_STATE;
      tx_data.CH7 = CH7_STATE;

      writeSBUS(tx_data);
      delayMicroseconds(600);
      ;

      START_Flag = 1;

      return;
    } else {
    }
  } else if (START_Flag == 1 && START == 0) {
    START_Flag = 0;
  }

  // DPAD BUTTONS
  // DPAD UP BUTTON - Fall Recover
  if (DPAD_UP == 1) {

    if (D_UP_Flag == 0) {
      CH5_STATE = MODE2;
      CH7_STATE = _LOW;
      tx_data.CH5 = CH5_STATE;
      tx_data.CH7 = CH7_STATE;

      writeSBUS(tx_data);
      delayMicroseconds(600);
      ;

      CH5_STATE = MODE2;
      CH7_STATE = _HIGH;
      tx_data.CH5 = CH5_STATE;
      tx_data.CH7 = CH7_STATE;

      writeSBUS(tx_data);
      delayMicroseconds(600);
      ;

      CH5_STATE = MODE2;
      CH7_STATE = _LOW;
      tx_data.CH5 = CH5_STATE;
      tx_data.CH7 = CH7_STATE;

      writeSBUS(tx_data);
      delayMicroseconds(600);
      ;

      D_UP_Flag = 1;

      return;
    }
  } else if (DPAD_UP == 0 && D_UP_Flag == 1) {
    D_UP_Flag = 0;
  } else {
  }

  // DPAD DOWN BUTTON - STAND Toggle
  if (DPAD_DOWN == true) {
    if (D_DOWN_Flag == 0) {

      if (HIGH_STAND_Flag == 0) {

        CH5_STATE = MODE1;
        CH8_STATE = _MID;
        tx_data.CH5 = CH5_STATE;
        tx_data.CH8 = CH8_STATE;

        writeSBUS(tx_data);
        delayMicroseconds(600);
        ;

        CH5_STATE = MODE1;
        CH8_STATE = _HIGH;
        tx_data.CH5 = CH5_STATE;
        tx_data.CH8 = CH8_STATE;

        writeSBUS(tx_data);
        delayMicroseconds(600);
        ;

        CH5_STATE = MODE1;
        CH8_STATE = _LOW;
        tx_data.CH5 = CH5_STATE;
        tx_data.CH8 = CH8_STATE;

        writeSBUS(tx_data);
        delayMicroseconds(600);
        ;

        D_DOWN_Flag = 1;
        HIGH_STAND_Flag = 1;

        return;

      } else if (HIGH_STAND_Flag == 1) {

        CH5_STATE = MODE1;
        CH8_STATE = _MID;
        tx_data.CH5 = CH5_STATE;
        tx_data.CH8 = CH8_STATE;

        writeSBUS(tx_data);
        delayMicroseconds(600);
        ;

        CH5_STATE = MODE1;
        CH8_STATE = _LOW;
        tx_data.CH5 = CH5_STATE;
        tx_data.CH8 = CH8_STATE;

        writeSBUS(tx_data);
        delayMicroseconds(600);
        ;

        CH5_STATE = MODE1;
        CH8_STATE = _MID;
        tx_data.CH5 = CH5_STATE;
        tx_data.CH8 = CH8_STATE;

        writeSBUS(tx_data);
        delayMicroseconds(600);
        ;

        D_DOWN_Flag = 1;
        HIGH_STAND_Flag = 0;

        return;
      }
    }
  } else if (DPAD_DOWN == 0 && D_DOWN_Flag == 1) {
    D_DOWN_Flag = 0;
  } else {
  }

  data.ch[0] = Rx;
  data.ch[1] = Ry;
  data.ch[2] = Ly;
  data.ch[3] = Lx;
  data.ch[4] = CH5_STATE; // CH5 default 992
  data.ch[5] = CH6_STATE; // CH6
  data.ch[6] = CH7_STATE; // CH7
  data.ch[7] = CH8_STATE;
  data.ch[8] = 1690;
  data.ch[9] = 154;

  sbus_tx.data(data);
  sbus_tx.Write();
}

class MyServerCallbacks : public BLEServerCallbacks {
  void onConnect(BLEServer *pServer) {
    deviceConnected = true;
    DEBUG_BLE_CONNECT();
  }

  void onDisconnect(BLEServer *pServer) {
    deviceConnected = false;
    DEBUG_BLE_DISCONNECT();

    START_Flag = 0;
    D_UP_Flag = 0;
    D_DOWN_Flag = 0;
    HIGH_STAND_Flag = 1;

    data_to_send[0] = 988;
    data_to_send[1] = 991;
    data_to_send[2] = 988;
    data_to_send[3] = 992;

    pServer->getAdvertising()->start();
    Serial.println("[BLE] 重启蓝牙广播");
  }
};

class MyCharacteristicCallbacks : public BLECharacteristicCallbacks {
  void onWrite(BLECharacteristic *pCharacteristic) {
    String value = pCharacteristic->getValue();

    if (value.length() > 0) {
      String data = String(value.c_str());

      // LED控制指令
      if (data.startsWith("command: ")) {
        String arg = data.substring(9);
        arg.trim();

        if (arg == "unlock") {
          data_to_send[4] = 1;
        } else if (arg == "forward") {
          // 前进
          data_to_send[0] = 988;
          data_to_send[1] = 1792;
          data_to_send[2] = 988;
          data_to_send[3] = 992;
        } else if (arg == "backward") {
          // 后退
          data_to_send[0] = 988;
          data_to_send[1] = 196;
          data_to_send[2] = 988;
          data_to_send[3] = 992;
        } else if (arg == "port") {
          // 左偏航
          data_to_send[0] = 988;
          data_to_send[1] = 991;
          data_to_send[2] = 432;
          data_to_send[3] = 992;
        } else if (arg == "starboard") {
          // 右偏航
          data_to_send[0] = 988;
          data_to_send[1] = 991;
          data_to_send[2] = 1548;
          data_to_send[3] = 992;
        } else if (arg == "left") {
          // 左横移
          data_to_send[0] = 432;
          data_to_send[1] = 991;
          data_to_send[2] = 988;
          data_to_send[3] = 992;
        } else if (arg == "right") {
          // 右横移
          data_to_send[0] = 1548;
          data_to_send[1] = 991;
          data_to_send[2] = 988;
          data_to_send[3] = 992;
        } else if (arg == "stop") {
          // 停下
          data_to_send[0] = 988;
          data_to_send[1] = 991;
          data_to_send[2] = 988;
          data_to_send[3] = 992;
        } else {
          Serial.println("[BLE] 错误的Command指令");
        }
      }
    }
  }
};

void setupBLE() {
  DEBUG_BLE_SETUP();

  BLEDevice::init("UnitreeGO2-BLE-Controller");

  pServer = BLEDevice::createServer();
  pServer->setCallbacks(new MyServerCallbacks());

  BLEService *pService = pServer->createService(SERVICE_UUID);

  pCharacteristic = pService->createCharacteristic(
      CHARACTERISTIC_UUID, BLECharacteristic::PROPERTY_READ |
                               BLECharacteristic::PROPERTY_WRITE |
                               BLECharacteristic::PROPERTY_NOTIFY);

  pCharacteristic->setCallbacks(new MyCharacteristicCallbacks());
  pCharacteristic->setValue("Hello World!");

  pCharacteristic->addDescriptor(new BLE2902());

  pService->start();

  BLEAdvertising *pAdvertising = BLEDevice::getAdvertising();
  pAdvertising->addServiceUUID(SERVICE_UUID);
  pAdvertising->setScanResponse(true);
  pAdvertising->setMinPreferred(0x06); // 有助于iPhone连接
  pAdvertising->setMinPreferred(0x12);
  BLEDevice::startAdvertising();

  Serial.println("[BLE] 蓝牙服务启动完成，等待连接...");
}

void setup() {
  Serial.begin(115200);
  sbus_tx.Begin();
  setupBLE();
}

void loop() {
  if (Serial.available() > 0) {
    String input = Serial.readStringUntil('\n');

    receivedCount = parseNumbers(input, numbers);
    if (receivedCount == 5) {
      for (int i = 0; i < 5; i++) {
        data_to_send[i] = numbers[i];
      }
    }
  }

  if (deviceConnected) {
#ifdef DEBUG
    Serial.printf("[BLE] CH0: %d, CH1: %d, CH2: %d, CH3: %d, CH4: %d\n",
                  data_to_send[0], data_to_send[1], data_to_send[2],
                  data_to_send[3], data_to_send[4]);
#endif
    sendCommand(data_to_send[0], data_to_send[1], data_to_send[2],
                data_to_send[3], data_to_send[4]);
  }
  delay(14);
}
